#include <fstream>
#include <iostream>
#include <boost/thread/thread.hpp>
#include <cstdio>
#include "vision_system.h"

VisionSystem::VisionSystem(
        SyncedLampCentroids *lamp_centroids) :
        lamp_detector_(kRestoredImageWidth, kRestoredImageHeight, false), lamp_centroids_(
                lamp_centroids) {
  camera_abstractor_.Initialize();

  fish_eye_restorer_function_lut_ = new FishEyeRestorerFunctionLUT(
          kHorizontalFieldOfView, camera_abstractor_.GetConfiguredImagesWidth(),
          camera_abstractor_.GetConfiguredImagesHeight(), kRestoredImageWidth,
          kRestoredImageHeight);

  captured_image_ = new uint32_t*[camera_abstractor_.GetConfiguredImagesHeight()];
          for(int i = 0; i < camera_abstractor_.GetConfiguredImagesHeight(); ++i)
  captured_image_[i] = new uint32_t[camera_abstractor_.GetConfiguredImagesWidth()];

  restored_image_ = new int*[kRestoredImageHeight];
  for (int i = 0; i < kRestoredImageHeight; ++i)
    restored_image_[i] = new int[kRestoredImageWidth];
}

VisionSystem::~VisionSystem() {
  for (int i = 0; i < camera_abstractor_.GetConfiguredImagesHeight(); ++i)
    delete captured_image_[i];
  delete captured_image_;

  for (int i = 0; i < kRestoredImageHeight; ++i)
    delete restored_image_[i];
  delete restored_image_;

  delete fish_eye_restorer_function_lut_;
}

void VisionSystem::operator()() {
  while (true) {
    camera_abstractor_.GetImage(captured_image_);

    for (int i = 0; i < kRestoredImageHeight; i++) {
      for (int j = 0; j < kRestoredImageWidth; j++) {
        PixelCoordinates *pix = fish_eye_restorer_function_lut_->AccessLut(i, j);

        if (pix != NULL) {
          int pixel_intensity = captured_image_[pix->x][pix->y];
          restored_image_[i][j] = (pixel_intensity > kThreshold) ? 1 : 0;
        } else {
          restored_image_[i][j] = 0;
        }
      }
    }

    vector<lamp> lamps_old = lamp_detector_.DetectLamps(restored_image_, 800, 200, 1.5, 0.6992);
    // printf("%d\n", (int) lamps_old.size());

    vector<player_point_2d> lamps_new;
    for (vector<lamp>::iterator it = lamps_old.begin(); it != lamps_old.end(); ++it) {
      player_point_2d lamp = { it->x / 100, it->y / 100 };
      // printf("lâmpada: %lf %lf\n", it->x, it->y);
      lamps_new.push_back(lamp);
    }
    // printf("\n");

    lamp_centroids_->set_lamp_centroids(lamps_new);

    boost::this_thread::sleep(boost::posix_time::milliseconds(200));
  }
}
